#!/usr/bin/env python3

import rospy
import sys
import os
import numpy as np
from sensor_msgs.msg import PointCloud2, PointField
import sensor_msgs.point_cloud2 as pc2
import struct

def publish_pcd_as_pointcloud(pcd_file):
    # 初始化ROS节点
    rospy.init_node('pcd_publisher', anonymous=True)
    
    # 创建发布器
    pub = rospy.Publisher('/pcd_cloud', PointCloud2, queue_size=1, latch=True)
    
    # 检查文件是否存在
    if not os.path.exists(pcd_file):
        rospy.logerr(f"文件不存在: {pcd_file}")
        return
    
    # 读取PCD文件
    rospy.loginfo(f"正在读取PCD文件: {pcd_file}")
    
    # 解析PCD文件头部
    with open(pcd_file, 'r') as f:
        header = {}
        for i in range(11):  # 通常PCD头部有11行
            line = f.readline().strip()
            if line.startswith('#'):
                continue
            key, value = line.split(' ', 1)
            header[key] = value
        
        # 获取点云大小
        width = int(header.get('WIDTH', 0))
        height = int(header.get('HEIGHT', 0))
        points_count = width * height
        
        # 获取数据格式
        data_format = header.get('DATA', 'ascii')
        
        # 读取点云数据
        points = []
        if data_format == 'ascii':
            for i in range(points_count):
                line = f.readline().strip()
                if not line:
                    continue
                values = line.split()
                if len(values) >= 3:  # 至少需要x,y,z坐标
                    x, y, z = float(values[0]), float(values[1]), float(values[2])
                    # 如果有强度值，也可以读取
                    intensity = float(values[3]) if len(values) > 3 else 0
                    points.append([x, y, z, intensity])
    
    # 创建PointCloud2消息
    msg = PointCloud2()
    msg.header.stamp = rospy.Time.now()
    msg.header.frame_id = "map"  # 设置坐标系
    
    # 定义点云字段
    fields = [
        PointField('x', 0, PointField.FLOAT32, 1),
        PointField('y', 4, PointField.FLOAT32, 1),
        PointField('z', 8, PointField.FLOAT32, 1),
        PointField('intensity', 12, PointField.FLOAT32, 1),
    ]
    
    # 将点云数据转换为PointCloud2消息
    msg = pc2.create_cloud(msg.header, fields, points)
    
    # 发布点云
    rate = rospy.Rate(1)  # 1Hz
    while not rospy.is_shutdown():
        pub.publish(msg)
        rospy.loginfo(f"已发布点云，包含 {len(points)} 个点")
        rate.sleep()

if __name__ == '__main__':
    if len(sys.argv) < 2:
        print("用法: view_pcd.py <pcd_file_path>")
        sys.exit(1)
    
    pcd_file = sys.argv[1]
    try:
        publish_pcd_as_pointcloud(pcd_file)
    except rospy.ROSInterruptException:
        pass 